#include <stdio.h>
#include <math.h>
#include "kalman.h"

/*
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = 1;
 *     H = 1; 
 *   and @q,@r are valued after prior tests.
 *
 *   NOTES: Please change A,H,q,r according to your application.
 *
 * @inputs  
 *   state - Klaman filter structure
 *   init_x - initial x state value   
 *   init_p - initial estimated error convariance
 * @outputs 
 * @retval  
 */
void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
    state->x = init_x;
    //state->p = init_p;
	//state->x = 0.2;
	state->p = 0.00001;
    state->A = 1;
    state->H = 1;
    //state->q = 0.9;//2e2;//10e-6;  /* predict noise convariance */
    //state->r = 0.3;//5e2;//10e-5;  /* measure error convariance */
	state->q = init_x;//2e2;//10e-6;  /* predict noise convariance */
    state->r = init_p;//5e2;//10e-5;  /* measure error convariance */
}

/*
 * @brief   
 *   1 Dimension Kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 * @retval  
 *   Estimated result
 */
float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict */
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;  /* p(n|n-1)=A^2*p(n-1|n-1)+q */

    /* Measurement */
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}

/*
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = {{1, 0.1}, {0, 1}};
 *     H = {1,0}; 
 *   and @q,@r are valued after prior tests. 
 *
 *   NOTES: Please change A,H,q,r according to your application.
 *
 * @inputs  
 * @outputs 
 * @retval  
 */
void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
    state->x[0]    = init_x[0];
    state->x[1]    = init_x[1];
    state->p[0][0] = init_p[0][0];
    state->p[0][1] = init_p[0][1];
    state->p[1][0] = init_p[1][0];
    state->p[1][1] = init_p[1][1];
    //state->A       = {{1, 0.1}, {0, 1}};
    state->A[0][0] = 1;
    state->A[0][1] = 0.1;
    state->A[1][0] = 0;
    state->A[1][1] = 1;
    //state->H       = {1,0};
    state->H[0]    = 1;
    state->H[1]    = 0;
    //state->q       = {{10e-6,0}, {0,10e-6}};  /* measure noise convariance */
    state->q[0]    = 10e-7;
    state->q[1]    = 10e-7;
    state->r       = 10e-7;  /* estimated error convariance */
}

/*
 * @brief   
 *   2 Dimension kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 *   state->x[0] - Updated state value, Such as angle,velocity
 *   state->x[1] - Updated state value, Such as diffrence angle, acceleration
 *   state->p    - Updated estimated error convatiance matrix
 * @retval  
 *   Return value is equals to state->x[0], so maybe angle or velocity.
 */
float kalman2_filter(kalman2_state *state, float z_measure)
{
    float temp0 = 0.0f;
    float temp1 = 0.0f;
    float temp = 0.0f;

    /* Step1: Predict */
    state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
    state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];
    /* p(n|n-1)=A^2*p(n-1|n-1)+q */
    state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
    state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
    state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
    state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];

    /* Step2: Measurement */
    /* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */
    temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
    temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
    temp  = state->r + state->H[0] * temp0 + state->H[1] * temp1;
    state->gain[0] = temp0 / temp;
    state->gain[1] = temp1 / temp;
    /* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
    temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];
    state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp); 
    state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);

    /* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */
    state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];
    state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
    state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];
    state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

    return state->x[0];
}


void main()
{
	kalman1_state kalman;
	kalman2_state kalman2;
	float state;
	float init_z[2];
	float *init_p[2];
	int i;
	double src_data[30] = {4.430786, 4.432327, 4.440811,4.444108,4.445648,4.446581,4.449042,
							4.450294, 4.451193,4.452515,4.454644,4.458331,4.458989,4.462853,
							4.462853,4.463066,4.463066,4.465615,4.46596,4.468468,4.469675,
							4.473649,4.475337,4.481661,4.484437,4.490076,4.492028,4.492616,
							4.513689,4.516462};
	
	double src_data2[30] = {1.430786, 2.432327, 3.440811,4.444108,5.445648,6.446581,7.449042,
							8.450294, 9.451193,10.452515,11.454644,12.458331,13.458989,14.462853,
							15.462853,16.463066,17.463066,18.465615,19.46596,20.468468,21.469675,
							22.473649,23.475337,24.481661,25.484437,26.490076,27.492028,28.492616,
							29.513689,30.516462};

	double x[62] = {2.539126,2.546689,2.508891,2.553887,2.541768,2.533591,2.571051,2.530253,2.562923,2.543315,2.543315,2.530653,2.570505,2.524798,
				2.572993,2.557545,2.520041,2.528175,2.547057,2.552768,2.509579,2.551065,2.555673,2.530524,2.535884,2.534815,2.543681,
				2.493674,2.531325,2.547425,2.526577,2.544379,2.512001,2.549040,2.526818,2.545293,2.542055,2.571055,2.539378,2.554990,
				2.522171,2.578157,2.556790,2.530834,2.564409,2.556652,2.563419,2.525953,2.591824,2.531374,2.531374,2.531374,2.531374,
				2.531374,2.531374,2.531374,2.531374,2.531374,2.531374,2.531374,2.531374,2.531374};

	double y[48] = {1.015123,1.009422,1.029001,1.030982,1.018198,1.059065,1.054612,1.048085,0.982232,1.022043,
					1.009422,1.045628,1.058952,1.054677,1.028388,1.020677,1.033453,1.013755,1.022043,1.018202,1.017104,1.022043,
					1.018202,1.005588,1.020182,1.023198,1.055597,1.010655,1.034352,1.050718,1.014360,0.996720,1.050081,1.047606,
					1.003608,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,1.015096,
					1.015096,1.015096};


	double negative_y[48] = {-1.015123,-1.009422,-1.029001,-1.030982,-1.018198,-1.059065,-1.054612,-1.048085,-0.982232,
							-1.022043,-1.009422,-1.045628,-1.058952,-1.054677,-1.028388,-1.020677,-1.033453,-1.013755,-1.022043,-1.018202,
							-1.017104,-1.022043,-1.018202,-1.005588,-1.020182,-1.023198,-1.055597,-1.010655,-1.034352,-1.050718,-1.014360,
							-0.996720,-1.050081,-1.047606,-1.003608,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,
							-1.015096,-1.015096,-1.015096,-1.015096,-1.015096,-1.015096};



	kalman1_init(&kalman, 0.5, 0.2);

	for (i = 0; i < 48; i++) {
		state = kalman1_filter (&kalman, negative_y [i]);
		printf("%f\n", state);
	}

	/*init_z[0] = 0.001;
	init_z[1] = 0.001;



	kalman2_init(&kalman2, init_z, init_p);
	state = kalman2_filter(&kalman2, 4.32341));
	printf("%f\n", state);*/
}
